/*
 * Javlov - a Java toolkit for reinforcement learning with multi-agent support.
 * 
 * Copyright (c) 2009 Matthijs Snel
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package net.javlov.world.phys2d;

import java.awt.Shape;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Map;

import net.javlov.Action;
import net.javlov.Agent;
import net.javlov.Environment;
import net.javlov.State;
import net.javlov.world.AgentBody;
import net.javlov.world.Body;
import net.javlov.world.CollisionListener;
import net.javlov.world.World;
import net.javlov.world.World.Continuous;

import net.phys2d.math.Vector2f;
import net.phys2d.raw.BroadCollisionStrategy;
import net.phys2d.raw.strategies.QuadSpaceStrategy;

/**
 * NOTE: THIS CLASS WAS IMPLEMENTED BASED ON A MODIFIED VERSION OF THE PHYS2D ENGINE, WHICH
 * COMES WITH THE JAVLOV SOURCE DISTRIBUTION BUT IS NOT INCLUDED IN SVN. THERE IS NO GUARANTEE
 * THAT THIS CLASS WILL WORK PROPERLY WITH THE ORIGINAL PHYS2D SOURCE, ALTHOUGH IT PROBABLY WILL.
 * 
 * @author Matthijs Snel
 *
 */
public class Phys2DWorld extends net.phys2d.raw.World implements World.Continuous {

	/**
	 * Maps agents to their bodies. Done this way because an agent is not allowed to have access
	 * to its own environment/body.
	 */
	protected Map<Agent, AgentBody> agentBodyMap;
	protected List<ConfigRule> configRules;
	
	private double width, height;

	protected float timestep;
	
	public Phys2DWorld(int iterations, float timestep) {
		this(iterations, new QuadSpaceStrategy(20,5), timestep);
	}
	
	public Phys2DWorld(int iterations, BroadCollisionStrategy strategy, float timestep) {
		super(new Vector2f(0, 9.81f), iterations, strategy);
		agentBodyMap = new HashMap<Agent, AgentBody>();
		configRules = new ArrayList<ConfigRule>();
		this.timestep = timestep;
	}
	
	public boolean addBody(Body b) {
		if ( b instanceof net.phys2d.raw.Body ) {
			super.add( (net.phys2d.raw.Body) b);
			return true;
		}
		return false;
	}
	
	public boolean add(Agent a, AgentBody body) {
		if ( addBody(body) ) {
			agentBodyMap.put(a, body);
			return true;
		}
		return false;
	}
	
	public void addConfigRule(ConfigRule rule) {
		configRules.add(rule);
	}
	
	//// GETTERS & SETTERS ////
	public Body getAgentBody(Agent a) {
		return agentBodyMap.get(a);
	}
	
	//TODO: get rid of this
	public double getTimeStep() {
		return timestep;
	}
	
	public double getWidth() {
		return width;
	}

	public void setWidth(double width) {
		this.width = width;
	}

	public double getHeight() {
		return height;
	}

	public void setHeight(double height) {
		this.height = height;
	}
	
	public List<Body> getObjects() {
		List<Body> ret = new ArrayList<Body>(bodies.size());
		for ( net.phys2d.raw.Body b : bodies )
			ret.add( (Phys2DBody) b );
		return ret;
	}
	
	/**
	 * Crude; only checks if the shape intersects the bounding area of any object
	 * @param s
	 * @return
	 */
	public List<Body> getIntersectingObjects(Shape s) {
		ArrayList<Body> objects = new ArrayList<Body>();
		for ( net.phys2d.raw.Body b : bodies ) {
			if ( s.intersects(b.getShape().getBounds2D()) )
				objects.add((Phys2DBody)b);
		}
		return objects;
	}
	
	/**
	 * Crude; only checks if the shape intersects the bounding area of any object
	 * @param s
	 * @return
	 */
	public boolean intersectsObject(Shape s) {
		for ( net.phys2d.raw.Body b : bodies )
			if ( s.intersects(b.getShape().getBounds2D()) )
				return true;
		return false;
	}
	
	public void disableBody(Body b) {
		Phys2DBody pb = (Phys2DBody) b;
		if ( !pb.disabled() ) {
			pb.setEnabled(false);
			removeBody(pb);
		}
	}
	
	public void enableBody(Body b) {
		Phys2DBody pb = (Phys2DBody) b;
		if ( pb.disabled() ) {
			pb.setEnabled(true);
			addBody(pb);
		}
	}
	
	///// ENVIRONMENT INTERFACE METHODS //////
	@Override
	public double executeAction(Action act, Agent a) {
		act.execute(a);
		return 0; //TODO: possibility for returning reward
	}

	@Override
	public State<double[]> getObservation(Agent a) {
		return agentBodyMap.get(a).getObservation(a);
	}

	/**
	 * Removes all bodies with a config rule from the world, then invokes doInit on all the config
	 * rules and places the bodies back in the world
	 */
	public void init() {
		arbiters.clear();
		//these for loops need to stay separate
		for ( ConfigRule rule : configRules )
			rule.preInit(this);
		for ( ConfigRule rule : configRules )
			rule.doInit(this);
		for ( Environment env : agentBodyMap.values() )
			env.init();
	}
	
	/**
	 * Invokes doReset on all config rules
	 */
	public void reset() {
		arbiters.clear();
		//these for loops need to stay separate
		for ( ConfigRule rule : configRules )
			rule.preReset(this);
		for ( ConfigRule rule : configRules )
			rule.doReset(this);
		for ( Environment env : agentBodyMap.values() )
			env.reset();
	}
	
	@Override
	public boolean remove(Agent a) {
		return (agentBodyMap.remove(a) == null ? false : true);
	}
	
	@Override
	public boolean removeBody(Body b) {
		if ( !(b instanceof net.phys2d.raw.Body) )
			return false;
		net.phys2d.raw.Body p2db = (net.phys2d.raw.Body) b;
		if ( bodies.contains(p2db) ) {
			super.remove(p2db);
			return true;
		}	
		return false;
	}
	
	@Override
	public boolean rotateBody(Body b, double angle) {
		((Phys2DBody) b).adjustRotation((float)angle);
		return true;
	}

	@Override
	public boolean translateBody(Body b, double dx, double dy) {
		((Phys2DBody) b).adjustPosition(new Vector2f((float)dx, (float)dy));
		return true;
	}

	@Override
	public int getObservationDim() {
		Iterator<Agent> it = agentBodyMap.keySet().iterator();
		return getObservation(it.next()).getData().length;
	}
}
